#include <iostream>
#include <fstream>
#include <iomanip>
#include "Frame.h"
#include "Kdtree_search.h"
#include "chrono"
#include "ransac.h"
#include "triangle.h"
#include "pose_estimate_3d2d.h"
#include "MapPoints.h"
#include "pcl_viewer.h"
#include "pose_estimation_2d2d.cpp"
using namespace std;
using namespace Eigen;

int main() {
    //test 特征匹配情况
    Relocation_Slam::ORBVocabulary *mpvocabulary;
    mpvocabulary = new Relocation_Slam::ORBVocabulary();
    bool bVocLoad = mpvocabulary->loadFromBinaryFile("/home/ubuntu/xd/RelocationSlam/ORBvoc.bin");

    cv::Mat img = cv::imread("/home/ubuntu/data/rgbd_dataset_freiburg3_long_office_household/1341847998.586740.png",CV_LOAD_IMAGE_UNCHANGED);
    //1.6642 -0.9328 1.5887 0.7066 0.4673 -0.2894 -0.4456
    std::cout<<888<<std::endl;
    Eigen::Isometry3d p_img = Eigen::Isometry3d::Identity();
    Eigen::Vector3d img_t = Eigen::Vector3d(1.6642,-0.9328,1.5887);
    Eigen::Quaterniond img_q(-0.4456,0.7066,0.4673,-0.2894);
    p_img.pretranslate(img_t);
    p_img.rotate(img_q);
    double id = 1341847998.586740;
    std::cout<<999<<std::endl;
    Relocation_Slam::Frame img_frame(mpvocabulary,img,p_img,id);

    double time = 0.1;
    Eigen::Isometry3d pose;

    cv::Mat img_11 = cv::imread ( "/home/ubuntu/data/rgbd_dataset_freiburg3_long_office_household/rgb/1341847990.274709.png", 1 );
    cv::Mat img_22 = cv::imread ( "/home/ubuntu/data/rgbd_dataset_freiburg3_long_office_household/rgb/1341847990.442575.png", 1 );

    Relocation_Slam::Frame frame1(mpvocabulary,img_11,pose,time);
    Relocation_Slam::Frame frame2(mpvocabulary,img_22,pose,time);

    vector<pair<int,int>> match_points;
    int matches = SelectByBoW(frame1,frame2,match_points,1);

    for(int i=0;i<matches;i=i+1){
        cv::circle(img_11,cv::Point(frame1.mvKeyPoints[match_points[i].first].pt.x,
                                frame1.mvKeyPoints[match_points[i].first].pt.y),8,CV_RGB(255,0,0),2);

        cv::circle(img_22,cv::Point(frame2.mvKeyPoints[match_points[i].second].pt.x,
                                frame2.mvKeyPoints[match_points[i].second].pt.y),8,CV_RGB(255,0,0),2);
    }
    cv::imshow("test", img_11);
    cv::imshow("test1", img_22);
//    cv::imwrite("/home/ubuntu/xd/img10(11).png",img_11);
//    cv::imwrite("/home/ubuntu/xd/img11(10).png",img_22);
    cv::waitKey(0);
    return 0;
}